#include "motor_hall.h"
#include "motor_foc.h"

// 霍尔状态到电角度的映射表 (Q12格式，0-16384对应0-360度)
// 假设电机极对数为7对极
static const int16_t hall_to_elec_angle[8] = {
    0,      // 000 - 无效状态
    2731,   // 001 - 60度
    5461,   // 010 - 120度
    8192,   // 011 - 180度
    10923,  // 100 - 240度
    13653,  // 101 - 300度
    16384,  // 110 - 360度
    0       // 111 - 无效状态
};

// 霍尔状态转换表，用于判断旋转方向
// 格式: [当前状态][下一状态] = 方向 (0:无效, 1:正转, 2:反转)
static const uint8_t hall_transition_table[8][8] = {
//   0  1  2  3  4  5  6  7
    {0, 0, 0, 0, 0, 0, 0, 0}, // 0 (000 无效状态)
    {0, 0, 2, 0, 1, 0, 0, 0}, // 1 (001)
    {0, 1, 0, 0, 0, 0, 2, 0}, // 2 (010)
    {0, 0, 0, 0, 2, 0, 0, 0}, // 3 (011)
    {0, 2, 0, 1, 0, 0, 0, 0}, // 4 (100)
    {0, 0, 0, 0, 0, 0, 1, 0}, // 5 (101)
    {0, 0, 1, 0, 0, 2, 0, 0}, // 6 (110)
    {0, 0, 0, 0, 0, 0, 0, 0}  // 7 (111 无效状态)
};
/**
 * @brief 初始化霍尔传感器处理模块
 * @param hall_handler 霍尔处理句柄
 * @param config 霍尔传感器配置
 */
void HALL_Init(HallHandler_t* hall_handler, HallConfig_t* config)
{
    if (hall_handler == NULL || config == NULL) {
        return;
    }
    
    hall_handler->config = *config;
    hall_handler->last_hall_state = 0;
    hall_handler->current_hall_state = 0;
    hall_handler->electrical_angle = 0;
    hall_handler->last_update_time = 0;
    hall_handler->direction = 0;
    hall_handler->speed_rpm = 0;
    hall_handler->revolution_count = 0;

	// 初始化历史缓冲区
    memset(hall_handler->hall_a_history, 0, sizeof(hall_handler->hall_a_history));
    memset(hall_handler->hall_b_history, 0, sizeof(hall_handler->hall_b_history));
    memset(hall_handler->hall_c_history, 0, sizeof(hall_handler->hall_c_history));
    hall_handler->history_index = 0;
}

/**
 * @brief 读取霍尔传感器状态
 * @param hall_handler 霍尔处理句柄
 * @return 霍尔状态值 (0-7)
 */
uint8_t HALL_ReadHallSensors(HallHandler_t* hall_handler)
{
    if (hall_handler == NULL) {
        return 0;
    }
    
    uint8_t hall_state = 0;
    
    // 读取原始霍尔传感器状态
    uint8_t raw_hall_a = (HAL_GPIO_ReadPin(hall_handler->config.hall_a_port, hall_handler->config.hall_a_pin) == GPIO_PIN_SET) ? 1 : 0;
    uint8_t raw_hall_b = (HAL_GPIO_ReadPin(hall_handler->config.hall_b_port, hall_handler->config.hall_b_pin) == GPIO_PIN_SET) ? 1 : 0;
    uint8_t raw_hall_c = (HAL_GPIO_ReadPin(hall_handler->config.hall_c_port, hall_handler->config.hall_c_pin) == GPIO_PIN_SET) ? 1 : 0;
    
    // 对每个传感器分别应用双边滤波器
    // 注意：需要为每个传感器维护独立的历史记录
    uint8_t filtered_hall_a = binary_bilateral_filter_u8(hall_handler->hall_a_history, raw_hall_a);
    uint8_t filtered_hall_b = binary_bilateral_filter_u8(hall_handler->hall_b_history, raw_hall_b);
    uint8_t filtered_hall_c = binary_bilateral_filter_u8(hall_handler->hall_c_history, raw_hall_c);
    
    // 更新历史缓冲区索引
    hall_handler->history_index = (hall_handler->history_index + 1) % HALL_FILTER_WINDOW_SIZE;
    
    // 组合滤波后的传感器状态
    if (filtered_hall_a) hall_state |= 0x04; // bit 2
    if (filtered_hall_b) hall_state |= 0x02; // bit 1
    if (filtered_hall_c) hall_state |= 0x01; // bit 0
    
    return hall_state;
}

/**
 * @brief 更新霍尔传感器状态和相关计算
 * @param hall_handler 霍尔处理句柄
 */
void HALL_Update(HallHandler_t* hall_handler)
{
    if (hall_handler == NULL) {
        return;
    }
    
    hall_handler->last_hall_state = hall_handler->current_hall_state;
    hall_handler->current_hall_state = HALL_ReadHallSensors(hall_handler);
    
    // 检查是否为无效状态
    if (hall_handler->current_hall_state == 0 || hall_handler->current_hall_state == 7) {
        // 保持之前的方向状态，不更新
        return;
    }
    
    // 检查霍尔状态是否发生变化
    if (hall_handler->current_hall_state != hall_handler->last_hall_state) {
        // 更新方向
        uint8_t transition = hall_transition_table[hall_handler->last_hall_state][hall_handler->current_hall_state];
        if (transition != 0) {
            hall_handler->direction = transition;
            
            // 如果是完整的一圈，更新转数计数
            if ((hall_handler->last_hall_state == 1 && hall_handler->current_hall_state == 6) ||
                (hall_handler->last_hall_state == 6 && hall_handler->current_hall_state == 1)) {
                if (hall_handler->direction == 1) {
                    hall_handler->revolution_count++;
                } else if (hall_handler->direction == 2) {
                    if (hall_handler->revolution_count > 0) {
                        hall_handler->revolution_count--;
                    }
                }
            }
        }
        
        // 计算速度
        uint32_t current_time = HAL_GetTick();
        if (hall_handler->last_update_time != 0) {
            uint32_t time_diff = current_time - hall_handler->last_update_time;
            if (time_diff > 0) {
                // 简化的速度计算 (RPM = 60000 / (time_diff * 6))
                // 假设每6个霍尔状态变化为一圈
                hall_handler->speed_rpm = 10000 / time_diff; // 简化计算
            }
        }
        hall_handler->last_update_time = current_time;
    }
    
    // 更新电角度
    hall_handler->electrical_angle = hall_to_elec_angle[hall_handler->current_hall_state];
}

/**
 * @brief 获取电角度
 * @param hall_handler 霍尔处理句柄
 * @return 电角度 (Q12格式)
 */
int16_t HALL_GetElectricalAngle(HallHandler_t* hall_handler)
{
    if (hall_handler == NULL) {
        return 0;
    }
    return hall_handler->electrical_angle;
}

/**
 * @brief 获取旋转方向
 * @param hall_handler 霍尔处理句柄
 * @return 方向 (0:未知, 1:正转, 2:反转)
 */
uint8_t HALL_GetDirection(HallHandler_t* hall_handler)
{
    if (hall_handler == NULL) {
        return 0;
    }
    return hall_handler->direction;
}

/**
 * @brief 获取速度(RPM)
 * @param hall_handler 霍尔处理句柄
 * @return 速度(RPM)
 */
uint16_t HALL_GetSpeedRPM(HallHandler_t* hall_handler)
{
    if (hall_handler == NULL) {
        return 0;
    }
    return hall_handler->speed_rpm;
}

/**
 * @brief 更新FOC输入数据
 * @param hall_handler 霍尔处理句柄
 * @param foc_controller FOC控制器
 */
void HALL_UpdateFOCInputs(HallHandler_t* hall_handler, FOC_Type* foc_controller)
{
    if (hall_handler == NULL || foc_controller == NULL) {
        return;
    }
    
    // 更新霍尔传感器输入
    foc_controller->inputs.b_hallA = (hall_handler->current_hall_state & 0x04) ? 1 : 0;
    foc_controller->inputs.b_hallB = (hall_handler->current_hall_state & 0x02) ? 1 : 0;
    foc_controller->inputs.b_hallC = (hall_handler->current_hall_state & 0x01) ? 1 : 0;
    
    // 更新机械角度(如果需要)
    // 根据极对数转换电角度到机械角度
    int32_t mechanical_angle = (int32_t)hall_handler->electrical_angle * 
                              foc_controller->motor_param.poles / 
                              foc_controller->speed_params.poles;
    if (mechanical_angle >= 16384) {
        mechanical_angle %= 16384;
    }
    foc_controller->inputs.a_mechAngle = (int16_t)mechanical_angle;
}
